;; Non-robust computational geometry functions
;; Copyright (C) 2022  Rocks Mazama
;;
;; This program is free software: you can redistribute it and/or modify
;; it under the terms of the GNU General Public License as published by
;; the Free Software Foundation, either version 3 of the License, or
;; (at your option) any later version.
;;
;; This program is distributed in the hope that it will be useful,
;; but WITHOUT ANY WARRANTY; without even the implied warranty of
;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
;; GNU General Public License for more details.
;;
;; You should have received a copy of the GNU General Public License
;; along with this program.  If not, see <https://www.gnu.org/licenses/>.

(ns mazama.compgeom.core)

;;;; NOTE
;;;;
;;;; 1. This software may fail if floating point values are used.
;;;;    This is because == is used to compare numbers and no special effort
;;;;    is made to determine if two floating point values are nearly equal.
;;;;
;;;; 2. This software assumes that:
;;;;    the x-axis increases to the right;
;;;;    the y-axis increases DOWNWARD.

(defn- lerp'
  "The one addtion, one multiplication version of lerp."
  [v0 delta t]
  (+ v0 (* t delta)))

;;; Reference: https://en.wikipedia.org/wiki/Linear_interpolation
(defn lerp
  "Linear interpolation."
  [v0 v1 t]
  (lerp' v0 (- v1 v0) t))

;;; Reference: https://eli.thegreenplace.net/2008/08/15/intersection-of-1d-segments"
(defn interval-intersect?
  "Determine if two 1-dimensional intervals intersect.
  Return true if and only if the two 1-dimensional closed intervals
  [s1 s2] and [t1 t2] intersect.
  Assumes s1 <= s2 and t1 <= t2."
  ([[s1 s2] [t1 t2]]
   (interval-intersect? s1 s2 t1 t2))
  ([s1 s2 t1 t2]
   (and (>= s2 t1) (>= t2 s1))))

(defn v+
  "Add two vectors."
  [[vx vy] [wx wy]]
  [(+ vx wx) (+ vy wy)])

(defn v-
  "Subtract two vectors."
  [[vx vy] [wx wy]]
  [(- vx wx) (- vy wy)])

(defn v*
  "Multiply a vector by a scalar."
  [k [vx vy]]
  [(* k vx) (* k vy)])

(defn v==
  "Are the two vectors, `v` and `w` ==?"
  [v w]
  (and (== (count v) (count w))
       (every? identity (map == v w))))

(defn magnitude-squared
  "Return the square of the magnitude of the vector."
  ([[vx vy]]
   (magnitude-squared vx vy))
  ([vx vy]
   (+ (* vx vx) (* vy vy))))

(defn magnitude
  "Return the magnitude of the vector."
  ([[vx vy]]
   (magnitude vx vy))
  ([vx vy]
   (Math/sqrt (magnitude-squared vx vy))))

(defn distance-squared
  "Return the square of the distance between two points."
  ([[ax ay] [bx by]]
   (distance-squared ax ay bx by))
  ([ax ay bx by]
   (magnitude-squared (- bx ax) (- by ay))))

(defn perp
  "Return the counterclockwise perpendicular vector with the same magnitude
  as the given vector."
  ([[vx vy]]
   (perp vx vy))
  ([vx vy]
   ;; Normally the y component is negated but we negate x because of downward y
   [vy (- vx)]))

(defn dot-product
  "Return the dot product of two 2d vectors."
  ([[vx vy] [wx wy]]
   (dot-product vx vy wx wy))
  ([vx vy wx wy]
   (+ (*' vx wx) (*' vy wy))))

(defn perp-dot-product
  "The perp-dot product, aka determinant, aka the signed area of the
  parallelogram defined by two 2d vectors, v and w.
  If w is counterclockwise from v (aka left) then the value is positive;
  if w is clockwise from v (aka right) then the value is negative;
  otherwise v and w are linearly dependent and the value is 0."
  ([[vx vy] [wx wy]]
   (perp-dot-product vx vy wx wy))
  ([vx vy wx wy]
   ;; Return the negative of the usual value to account for downward y
   (- (*' wx vy) (*' vx wy))))

;;; Reference:
;;; https://www.math.ucla.edu/~baker/149.1.02w/handouts/b_vectors.pdf, Section 7
(defn vector-angle
  "Compute the signed angle, in radians, between two vectors, v and w.
  A counterclockwise angle from v to w is positive,
  a clockwise angle is negative."
  ([[vx vy] [wx wy]]
   (vector-angle vx vy wx wy))
  ([vx vy wx wy]
   (Math/atan2 (perp-dot-product vx vy wx wy) (dot-product vx vy wx wy))))

(defn vector-orient2d
  "Return the relative orientation of two 2d vectors.
  If the vectors v and w are linearly dependent then return 0,
  otherwise imagining the tails of the vectors to be superimposed,
  if w is closer to v in a clockwise direction return -1;
  if w is closer to v in a counterclockwise direction return 1."
  ([[vx vy] [wx wy]]
   (vector-orient2d vx vy wx wy))
  ([vx vy wx wy]
   (let [det (perp-dot-product vx vy wx wy)]
     (cond (neg? det) -1
           (pos? det) 1
           :else 0))))

(defn point-orient2d
  "Return the relative orientation of three 2d points.
  If the points a, b, and c occur in a clockwise order return -1;
  if the points occur in a counterclockwise order return 1;
  otherwise the points are colinear and return 0."
  ([[ax ay] [bx by] [cx cy]]
   (point-orient2d ax ay bx by cx cy))
  ([ax ay bx by cx cy]
   (vector-orient2d (- bx ax) (- by ay) (- cx ax) (- cy ay))))

(defn on-or-left-of-line-segment?
  "Does the point `d` lie on or to the left of the directed line segment `a`->`b`?"
  [[ax ay] [bx by] [dx dy]]
  (case (point-orient2d ax ay bx by dx dy)
    -1 false
    0 (and (or (<= ax dx bx) (>= ax dx bx))
           (or (<= ay dy by) (>= ay dy by)))
    1 true))

(defn- incircle-solid
  "Return a positive value if the point pd lies inside the
  circle passing through pa, pb, and pc; a negative value if
  it lies outside; and zero if the four points are cocircular.
  NOTE: The points pa, pb, and pc must be in counterclockwise
  order, or the sign of the result will be reversed. See:
  http://www.cs.cmu.edu/afs/cs/project/quake/public/code/predicates.c"
  [[pax pay] [pbx pby] [pcx pcy] [pdx pdy]]
  (let [adx (- pax pdx)
        ady (- pay pdy)
        bdx (- pbx pdx)
        bdy (- pby pdy)
        cdx (- pcx pdx)
        cdy (- pcy pdy)
        abdet (- (*' adx bdy) (*' bdx ady))
        bcdet (- (*' bdx cdy) (*' cdx bdy))
        cadet (- (*' cdx ady) (*' adx cdy))
        alift (+ (*' adx adx) (*' ady ady))
        blift (+ (*' bdx bdx) (*' bdy bdy))
        clift (+ (*' cdx cdx) (*' cdy cdy))]
    ;; Return the negative of the usual value to account for downward y
    (- (+' (*' alift bcdet) (*' blift cadet) (*' clift abdet)))))

(defn incircle
  "Return a positive value if the point pd lies inside the
  circle passing through pa, pb, and pc; a negative value if
  it lies outside; and zero if the four points are cocircular.
  NOTE: The points pa, pb, and pc must be in counterclockwise
  order, or the sign of the result will be reversed. See:
  http://www.cs.cmu.edu/afs/cs/project/quake/public/code/predicates.c
  EXTENSION:
  One of the points pa, pb, or pc can be nil which represents a ghost vertex
  at infinity. See:
  Section 3.4: http://people.eecs.berkeley.edu/~jrs/meshpapers/delnotes.pdf"
  ([[pa pb pc] pd]
   (incircle pa pb pc pd))
  ([pa pb pc pd]
   (cond
     (nil? pa) (if (on-or-left-of-line-segment? pb pc pd) 1 -1)
     (nil? pb) (if (on-or-left-of-line-segment? pc pa pd) 1 -1)
     (nil? pc) (if (on-or-left-of-line-segment? pa pb pd) 1 -1)
     :else (incircle-solid pa pb pc pd))))

(defn foot
  "Return the parameter of the foot of the point P on the parametric line
  defined by the point A and non-zero vector V."
  ([[px py] [ax ay] [vx vy]]
   (foot px py ax ay vx vy))
  ([px py ax ay vx vy]
   (let [pax (- px ax)
         pay (- py ay)]
     (/ (dot-product pax pay vx vy) (dot-product vx vy vx vy)))))

(defn foot-distance
  "Return the signed distance of the point P to the parametric line
  defined by the point A and non-zero vector V.
  The returned value is positive if P is to the left of V from A."
  ([[px py] [ax ay] [vx vy]]
   (foot-distance px py ax ay vx vy))
  ([px py ax ay vx vy]
   (let [pax (- px ax)
         pay (- py ay)]
     (/ (perp-dot-product vx vy pax pay) (magnitude vx vy)))))

(defn parametric-line
  "Return the parametric equation for a line defined by two non-equal points.
  The equation applied to 0 returns [x1 y1];
  the equation applied to 1 returns [x2 y2];
  the equation applied to 1/2 returns the midpoint, etc.
  The equation applied to a point returns the parameter of the foot
  of the point.
  The equation without argument returns the input points and vector."
  ([[x1 y1] [x2 y2]]
   (parametric-line x1 y1 x2 y2))
  ([x1 y1 x2 y2]
   (let [dx (- x2 x1)
         dy (- y2 y1)]
     (fn
       ([] [x1 y1 x2 y2 dx dy])
       ([t] [(lerp' x1 dx t) (lerp' y1 dy t)])
       ([x y] (foot x y x1 y1 dx dy))))))

(defn general-line
  "Return the general form coefficients for the line defined by the point `p` = [x y] and the vector `v` = [vx vy].
  General form is ax + by + c = 0
  https://en.wikipedia.org/wiki/Linear_equation#Two-point_form"
  [[x y] [vx vy]]
  [(- vy)
   vx
   (- (* x vy) (* vx y))])

(defn line-intersection
  "Return the intersection of two general form lines, `l1` = [a1 b1 c1] and `l2` = [a2 b2 c2].
  Return nil if the lines are parallel.
  https://en.wikipedia.org/wiki/Cramer%27s_rule#Explicit_formulas_for_small_systems"
  [[a1 b1 c1] [a2 b2 c2]]
  (let [D (- (* a1 b2) (* a2 b1))]
    (when-not (zero? D)
      [(/ (- (* b1 c2) (* b2 c1)) D)
       (/ (- (* a2 c1) (* a1 c2)) D)])))
